#include "data_kalman_filter.h"

void DATA_KalmanFilter_Init(
    DATA_KalmanFilter *kf,
    const float x,
    const float predict_error_cov,
    const float process_noise_cov,
    const float measure_noise_cov
) {
    kf->x = x;
    kf->k = 0;
    kf->predict_error_cov = predict_error_cov;
    kf->process_noise_cov = process_noise_cov;
    kf->measure_noise_cov = measure_noise_cov;
}

void DATA_KalmanFilter_Predict(DATA_KalmanFilter *kf) {
    // 状态预测（假设系统模型为x = x，即没有控制输入）
    kf->x = kf->x;
    // 协方差预测（P = P + Q）
    kf->predict_error_cov += kf->process_noise_cov;
}

void DATA_KalmanFilter_Update(DATA_KalmanFilter *kf, const float z) {
    // 计算卡尔曼增益
    kf->k = kf->predict_error_cov / (kf->predict_error_cov + kf->measure_noise_cov);
    // 更新状态估计
    kf->x = kf->x + kf->k * (z - kf->x);
    // 更新协方差估计
    kf->predict_error_cov = (1 - kf->k) * kf->predict_error_cov;
}
